/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;

import java.lang.*;
import edu.wpi.first.wpilibj.communication.FRCControl;
import edu.wpi.first.wpilibj.communication.FRCControlData;
import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class PLLTest extends IterativeRobot {

    Jaguar leftJag, rightJag;
    Encoder leftEnc, rightEnc;
    DriverStationLCD dslcd;
    AnalogModule analog;
    PIDController pid;
    EncoderDifference diff;
    Joystick leftStick, rightStick;
    static final int midPWM = 127;
    static final int midAnalog = 511;
    static final double speedStep = .02;
    int targetEncoderValue=0;
    boolean controlledDriving=false;
    int inchesToMove=24;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
	  //assume left side is reversed side
	  rightStick = new Joystick(2);
	  leftStick = new Joystick(1);
	  rightJag = new Jaguar(4, 2);
	  leftJag = new Jaguar(4, 1);
	  leftEnc = new Encoder(1, 2);
	  rightEnc = new Encoder(3, 4, true);
	  diff = new EncoderDifference(rightEnc, leftEnc);
	  //pid = new PIDController(.005, .001, 0, diff, leftJag);
	  pid = new PIDController(.0025, .001, 0, diff, leftJag);
	  dslcd = DriverStationLCD.getInstance();
	  leftEnc.setDistancePerPulse(.001);
	  rightEnc.setDistancePerPulse(.001);
	  analog = AnalogModule.getInstance(1);
	  pid.setOutputRange(-1, 1);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopInit(){

    }

    public void teleopPeriodic() {
	  leftEnc.start();
	  rightEnc.start();
	  leftEnc.reset();
	  rightEnc.reset();
	  //leftJag.set(0);
	  //rightJag.set(0);
	  //pid.enable();
	  int frameCount = 0;
	  boolean runOnceInit = true, runOnceDisable = false;
	  while (isOperatorControl()) {
		if (isEnabled()) {
		    //rightJag.set((analog.getVoltage(3)/2.5)-1);
		    //rightJag.set(leftStick.getY());
		    //rightEnc.get();
		    //leftJag.pidWrite(????);
		    if(rightStick.getRawButton(10)||rightStick.getRawButton(11)){
			  leftEnc.reset();
			  rightEnc.reset();
			  pid.reset();
		    }

		    if (rightStick.getRawButton(2)) {
			  if (runOnceInit) {
				//leftEnc.reset();
				//rightEnc.reset();
				leftJag.set(0);
				rightJag.set(0);
				pid.enable();
				//pid.reset();
				frameCount = 0;
				runOnceInit = false;
			  }
			  rightJag.set(rightStick.getY());
		    }
		    else if(leftStick.getRawButton(1)){
			  if(!controlledDriving){
				targetEncoderValue=leftEnc.get()+(int)(inchesToMove*14.96);
				controlledDriving=true;
			  }
		    }
		    if(controlledDriving){
			  rightJag.set(.2);
			  leftJag.set(-.2);
			  if(leftEnc.get()>=targetEncoderValue)controlledDriving=false;
		    }
		    else{
			  if (!runOnceInit) {
				//leftEnc.reset();
				//rightEnc.reset();
				pid.disable();
				frameCount = 0;
				runOnceInit = true;
			  }

	 		  rightJag.set(rightStick.getY());
			  leftJag.set(-leftStick.getY());
		    }
		    if (frameCount % 500 == 0) {
			  dslcd.println(DriverStationLCD.Line.kUser4, 1, "R:" + rightEnc.get() + " L:" + leftEnc.get() + "   ");
			  dslcd.println(DriverStationLCD.Line.kUser5, 1, "Err: " + diff.pidGet() + "   ");
			  dslcd.updateLCD();
			  frameCount = 0;
		    }
		    frameCount++;
		}
	  }
	  //leftEnc.

	  pid.disable();
	  rightEnc.stop();
	  leftEnc.stop();


    }
}
